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Abstract: This paper presents a state of the art of the estimation algorithms dealing with 
Out-of-Sequence (OOS) measurements for non-linearly modeled systems. The state of 
the art includes a critical analysis of the algorithm properties that takes into account the 
applicability of these techniques to autonomous mobile robot navigation based on the fusion 
of the measurements provided, delayed and OOS, by multiple sensors. Besides, it shows a 
representative example of the use of one of the most computationally efficient approaches 
in the localization module of the control software of a real robot (which has non-linear 
dynamics, and linear and non-linear sensors) and compares its performance against other 
approaches. The simulated results obtained with the selected OOS algorithm shows the 
computational requirements that each sensor of the robot imposes to it. The real experiments 
show how the inclusion of the selected OOS algorithm in the control software lets the robot 
successfully navigate in spite of receiving many OOS measurements. Finally, the comparison 
highlights that not only is the selected OOS algorithm among the best performing ones of 
the comparison, but it also has the lowest computational and memory cost. 

Keywords: autonomous mobile robots; location estimation; out-of-sequence; extended 
Kalman filter 
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1. Introduction 

Autonomous mobile robots require to be localized, either in a local or global frame, in order to 
successfully perform different navigation tasks. With that purpose, they are usually equipped with 
(1) multiple sensors that provide redundant or complementary information about the robot location 
and (2) a localization module that is responsible for estimating on-line the robot location fusing the 
information provided by the sensors. The calculated location estimates are used in other modules of 
the robot control software that are in charge of deciding how the robot should act next. This makes the 
control signals applied to the robot actuators highly dependent on the location estimates. Therefore, it is 
extremely important to estimate the robot location correctly and efficiently. 

In order to achieve both objectives, the location module can implement any of the sequential 
estimators that can deal with the uncertainty and characteristics associated to the robot dynamics and 
sensors, such as the Kalman, Information or Particle Filter (KF, IF, PF [1-4]). When we apply their 
basic formulations to the robot estimation problem, they sequentially estimate the current time robot 
location based on the current time measurements and previous time location estimate. Therefore, when 
the localization module implements the basic formulation of these filters (whose behavior is schematized 
in Figure 1(a), representing the dependency of the location estimate on the current time measurements 
with the arrows and on the previous time location estimate with the arcs), the location module requires to 
have all the measurements associated to the current time step before obtaining the estimate related to it. 

Figure 1. Out-Of-Sequence Problem, (a) Non-delayed data; (b) 1-step lag delay data; 
(c) N-step lag delay data. 
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However, the measurements often arrive delayed to the localization module, due to multiple factors 
such as the physical distribution of the sensors in some robotic problems, the communication network 
used to send information from the sensing modules to the localization one, and/or the time used to 
pre-process the raw measurements and extract the useful information that is sent to the localization 
module. The most difficult scenario appears when the delays and the sequence of the arrival of 
information to the localization module are not fixed, constituting the Out-Of-Sequence Problem 
(OOSP) [5]. The two main types of OOSP that can affect the sequential estimator of the localization 
module are presented in Figures 1(b) and 1(c). The former is the delayed 1-step lag problem where the 
time stamp of the delayed measurements (#2 and #5) falls between the time stamps of the two previously 
arrived measurements (#1 and #3, and #4 and #6). The later is the general OOS n-step lag problem, 
where the measurements arrive delayed and OOS without any type of restrictions (#1 causes a 3-step lag 
OOSP, #2 causes a 1-step lag OOSP, and #4 causes a 2-step lag OOSP). 
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In order to deal with the measurement arrival delays, the localization module can basically implement 
four different solutions. The first, which consists on discarding the delayed measurements, is the easiest 
solution because it only requires to check the time stamp of the measurements. However, it is only 
useful for systems with spurious delayed measurements, because rejecting measurements increases the 
uncertainty of the robot location, affecting the stability and reducing the reliability of the robot control 
system [6-9]. The second approach, based on postponing the estimation until all the measurements are 
available, requires some extra memory to buffer the already received measurements and some knowledge 
about the sensors sampling rates and/or measurements maximum delays to decide when the estimation 
can be carried out [10]. However, it is only valid when the delays are small as it happens in [11], 
because the robot control system usually obtains the actions based on the current pose estimation and 
cannot wait to receive an estimate that is significantly delayed. The third approach consists in storing 
for all time instances the location estimates and measurements, rolling back to the time step associated 
with the time stamp of the measurement that has just arrived, and restarting the estimation process from 
that point. Therefore, it is the brute-force solution, used in [12-14], that lets the localization module 
obtain the same estimates as if it had received all the data without delays, at the expenses of increasing 
significantly the computational and memory requirements of the estimation algorithm. Finally, there is a 
fourth solution that avoids the computational burden of the brute-force approach and that is implemented 
in the localization module of the non-linearly modeled autonomous robot presented in this article. It 
consists in using (or developing) an OOS version of the filter that would have been used if there were 
no delays. 

The estimation community has developed many OOS algorithms during the last decade in order to 
fulfill the requirements of an increasing number of sensor networks and tracking networked systems. 
Among the big number of possibilities, the choice of OOS algorithm to implement in the localization 
module of the robot depends on (1) the types of dynamic and sensorial models associated to the robot, 
and on (2) the nature of the out of sequence problems that the localization module faces. For instance, 
those autonomous robots whose dynamic and sensorial behaviors are modeled as linear systems with 
additive Gaussian noise can implement any of the big quantity of OOS Kalman or Information Filter 
variants (see [15] for a comprehensive review and comparison of centralized OOS KFs and OOS IFs, 
and [16-21] for decentralized OOS KFs). The smaller number of OOS variants for non-linearly modeled 
systems and the assumptions made during their development reduces the possibilities that exist for 
non-linearly modeled autonomous mobile robots. This work analyses the different possibilities, studies 
the performance of one of the most generic and computationally efficient existing approaches (which is 
adapted to take into account some peculiarities of a non-linearly modeled autonomous robot localization 
problem) and compares the selected approach against others. 

This paper is organized as follows. Section 2 presents the analysis of the characteristics of the existing 
OOS filters for non-linear systems and studies the tracking non-linear modeled mobile object problems 
that have been tested with the analyzed OOS filters and that are close to the problem of estimating the 
location of an autonomous mobile robot with controlling purposes. In order to illustrate how to apply 
one of these techniques within the control software of a real robot, Section 3 analyzes the dynamic 
and sensors models of one of our robots, explains the general and efficient algorithm that has been 
adapted to use in the localization module of the robot control software, analyzes the performance of this 
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algorithm under simulated and real data, and compares its performance against some other applicable 
OOS techniques. Finally, some conclusions are presented in Section 4. 

2. State of the Art 

In this section we analyze the different OOS filters for non-linear systems that haven been developed 
during the last years as well as the autonomous robotic and tracking examples that are found in the 
literature and that already consider the OOSP. 

2. 1. OOS Algorithms for Non-Linear Systems 

The algorithms analyzed in this section are those that can estimate the location x tk of a system at 
time tk given (1) the measurements z s>ti provided by s = 1 : S different sensors at different time stamps 
ti\ and (2) the probability models p(x tk \x ti , u tk)ti ) and p(z s :tk \x tk ) that establish that the location x tk 
at tk probabilistically depends on the location x ti at a previous time step U and on the control signal 
u tk ti applied between and t k , and that the measurement z s ttk probabilistically depends on the location 
x tk . In our analysis, we also include those OOS algorithms whose dynamic behavior is only modeled 
as p{x tk \x ti ) {i.e., without control signals), because its extension to control systems is not difficult, and 
those systems that consider the control signal are more general that those systems that don't. 

One important type of system that fulfills those requirements, and which can be used to model 
different types of autonomous mobile robots, is the group of non-linear systems with Gaussian 
probabilistic density functions that can be modeled with the expressions in Equation (1), where N a (b, C) 
represents the normal distribution of variable a with mean b and covariance C, /(■) and h s (-) are the 
transition and measurement functions that capture the dependency relationships among the variables, 
and Qt k ,u and R s ,t k stand for the covariance matrices in the transition and measurement models. 

p{x tk \x tv u tkM ) = N x (fix^Ut^t^U), Q tktti ) 

(1) 

P[z s ,t h M = N Zs tk (h s [x tk ,t k ), R s , tk ) V s = 1 : S 

An equivalent representation of the system is presented in Equation (2), where u tk )t . and fi S:tk are 
random variables with zero mean and covariances Qt k ,u an d R s ,t k that represent the additive noise of 
the transition/measurements non-linear models /(•) and h s (-). 

x t k = f( x U,Ut k ,U,tk,ti) + V tk ,U 

z S;tk = h s (x tk ,t k )+fj, s>tk Vs = l:S 

In order to estimate the value of x tk for the non-linear problem presented in either Equation (1) 
or Equation (2), we can approximate p(x tk \z 1 , SUk: u to . tk ) as N Xtk (x tk \t h , Pt k \t J usin § the Linearized 
Kalman or Information Filters (LKF/LIF), the Extended Kalman or Information Filter (EKF/EIF), 
the Unscented Kalman Filter (UKF), or the Ensemble Kalman Filter (EnKF) [2,3,22,23]. When the 
noise and or relationships are not necessarily Gaussian and/or linear (i.e., when p(x tk \x ti ,u tkjti ) and 
p(z s j k \x tk ) are generic probability density functions), the estimation problem can be solved using 
sequential Monte Carlo techniques, such as the Sampling Importance Resampling PF (SIR, [4]) or the 
Unscented PF (UPF, [24]) that approximate p(x 0 . tk \z 1 , Syto , k , u to:tk ) using the point-mass distribution 
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Hj=\W { - j) 5{xt 0 :t k -xf Q l tk ), or the Marginal PF (MPF, [25]) that approximates p(x tk \zi., s ,t 0:k , u to . tk ) 
using the point-mass distribution 2~2f=i w^8(x tk — xf k ). 

The basic formulations of the LKF/LIF, EKF/EIF, UKF, EnKF, and PF cannot deal with the OOSP 
Therefore, new OOS versions of these filters have been developed in the last decade. We briefly describe 
them in the following paragraphs, referring to those OOS algorithms that appear in the same paper as 
others by the paper number and its habitual nomenclature in the OOS literature (for instance, [5]-Al 
represents the OOS algorithm Al by Bar-Shalom [5]). 

• OOS Linearized KF/IF ([26]): Linearizing the system models makes possible the direct use of the 
OOS KF/IF in the linearized system. Although this approach is applied with success in [26], where 
the OOS 1-step lag [5]-Al KF is used for tracking autonomous vehicles with visual information 
only delayed 1 time step, it can produce erroneous results when the robot dynamic and sensorial 
models have strong non-linearities. 

• OOS Extended KF/IF ([27-31]): They extend some OOS KF/IF to make them deal with the 
non-linearities of the problem: 

- [27,28]-EBl present the extension of the retrodiction OOS KF in [32]-Bl to the non-linear 
case. This OOS EKF can work with non-linear measurement functions h s (x tk ,tk), although its 
transition function f(xt z ,u tk!t .,tk,ti) has to be linear (F tktti x ti + Ut^tJ, due to the retrodiction 
(backward propagation) operation used in [32]-Bl. Besides, [27,28]-EBl only deals with the 
1-step lag OOSP and can produce erroneous results when the robot dynamic and sensorial 
models have strong non-linearities. 

- [29] study the performance of extended versions of the retrodiction OOS KF in [33]-All 
and [33]-Bll, and the forward OOS KF on [34]&[29]-FPFD in a system with linear transition 
and non-linear measurement models. Although the linearity of the transition model is a 
requirement of the retrodiction step only in [33]-All and [33]-Bll, [34]&[29]-FPFD will only 
work properly if the non-linear transition model can be used to calculate properly the mean value 
of x tk = f{xt i - l Ut k ,t i -,tk- l t i ) for any At = tk — U. Besides, the Extended OOS KFs presented 
in [29], and called [29]-EAll, [29]-EBll, [29]-EFPFD hereafter, can produce erroneous results 
when the robot dynamic and sensorial models have strong non-linearities. 

- [30,31]-EIFAsyn extends the forward OOS IF presented in [30,31]-IFAsyn to work with 
systems with linear and/or non-linear transition and sensorial models. Besides, in order to 
overcome the difficulties that the former non-linear OOS algorithms face in systems with strong 
non-linearities, it distinguish two types of non-linear sensors: those whose information needs 
to be recalculated when older measurements arrive at the localization module and those whose 
information do not need to be recalculated. 

• OOS Unscented KF ([35]): The OOS Unscented KF in [35] combines the retrodiction step of 
the OOS KF in [5] with the measurement update of the Unscented KF [22]. It requires a linear 
transition model (due to the retrodiction step) and it only deals with the 1-step lag OOSP. 

• OOS Ensemble KF ([36]): The OOS Ensemble KF in [36] modifies the Ensemble KF [23] to let 
it work with N-step lag multisensor OOSP, by fusing the estimates obtained by S independent 
Ensemble KFs, each of them including a (1) backward propagation step that calculates the location 
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values at the measurement time stamp linearly interpolating the location values of the previous and 
posterior time steps and (2) a new set of assimilation operations that let it deal with the delays of 
the measurements. 

• OOS PF ([37-45]): They modify the non-OOS SIR, UPF and MPF to let them work with the 
OOS measurements z s ,t m on ly m systems with Gaussian probability models (Equation (1) or 
Equation (2)). That is, although the basic versions of these PFs do not require Gaussian probability 
models, its OOS counterparts, described next, need it due to different assumptions made during 
their development: 

- The OOS SIR PFs ([37-40] and [42]-A) and the OOS UPF ([41]) update the weights of the 
particles making oc w^p{z Sttm \x^). The value of is either (1) the value of the 
location of the j-th particle at time t m when the PF has already associated a measurement 
for the time stamp of the new z a tm ; or (2) the value sampled from a probability distribution 
q(x tm \xY a , xY i z s ,t m ), different for each type of PF, that takes into account the values of the 
particles at t a and U, where t a and U are the closest lower and higher time stamps to t m of 
already assimilated measurements. The sampling distribution in [37-39], q(x t J\x^ , x$), does 
not consider the current measurement and exploits the properties of a Gaussian linear transition 
model. The value of in [40] is obtained linearly interpolating the location values of and 
x$ . The sampling distribution in [41], q UKF (x t J\x^ ,x[^ , z Sttm ), is obtained with an UKF 
whose prediction step has the same linearity requirement as the q(x tm \xYJ , xY) in [37-39] and 
whose update step incorporates z Sttm using the unscented transformation. Finally, the sampling 
distribution in [42]-A, qEKp(x tm \x^J , x$), is obtained with an EKF with a prediction step 
from t a to t m that uses the system Gaussian transition model, and an update step that treats the 
transition from t m to as a measurement. 

- The OOS MPFs ([42]-B# and [43-45]) update the weights of the particle for the delayed 
measurements making oc p(z B ,t m \ x t > z i-.s,t 0 :t k ) w ^ ■ Besides, they update the values of 
the non-delayed measurements with the usual SIR step, which creates PFs that acs as SIR when 
t m > tk and as MPFs when t m < t^. However, as the SIR step is equivalent to the MPF step 
with transition prior [25], we can classify [42]-B# and [43-45] as purely MPFs. The MPFs 
in [42] obtain p(z s :tm \x^, Zi :Stto: t k ) exploiting the property that this probability is dependent 
on the smoothed density p(x tm s,t 0 -.t k ), which can be approximated using a Fixed-Point 
Extended Kalman Smoother ([42]-Bl), a Fixed-Point Unscented Smoother ([42]-B2) or a 
Fixed-Point Particle Smoother ([42]-B3). The p{Zs,t m \ x ¥l-i z i-s,t 0 -t k ) m [43] is approximated by 
p(z s ,t m \ x tH)> obtaining with the retrodiction step in [5]. Finally, the MPFs in [44] and [45] 
improve the efficiency of the MPFs in [42] using some additional mechanisms to select which 
of the delayed measurements should be assimilated. 

The main characteristics of the OOS non-linear filters are summarized in Table 1 . The first column 
(ALG) identifies the algorithm, the second (Group) the group they belong to, the third (Extending) the 
algorithm they are extending/modifying, and the fourth (Support) the core idea or distribution function 
that lets the algorithm obtain the location associated to the time stamp of the delayed measurement and 
assimilate the measurement information into the current location estimate. The fifth (Trans. Model) 
and sixth (Meas. Model) show which types of Gaussian Transition and Measurement Models the 
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filters are restricted to: L stands for linear, NL for Non-Linear, and Lzd for linearized. The seventh 
column (OOSP) identifies if they are prepared to deal with the 1-step lag or N-step lag OOSP. Finally, 
the eighth column (Extras) shows other important additional information: [30,31]-EIFAsyn optionally 
recalculate the sensorial information of some of the already assimilated measurements whose time stamp 
is bigger than the just arrived one to avoid the problems associated to systems with strong non-linearities, 
the MCMC step in [38,39] increments the diversity of the old history values to reduce the problems 
associated with particles depletion, and the check diversity mechanisms in [42]-B# and [44,45] are used 
to decide whether an OOS z Sttm should be assimilated or not. 

Table 1. OOS Filters for Gaussian Non-Linear Systems. 



ALG 


Group 


Extending 


Support 


Trans. Model 


Meas. Model 


OOSP 


Extras 


[26] 


LKF 


[5]-Al 


Retrodiction 


Lzd 


Lzd 


1 step-lag 




[27,28]-EBl 


EKE 


[32]-Bl 


Retrodiction 


L 


L/NL 


N step-lag 




[29]-EAl 


EKE 


[33]-All 


Retrodiction 


L 


L/NL 


N step-lag 




[29]-EBl 


EKF 


[33]-Bll 


Retrodiction 


L 


L/NL 


N step-lag 




[29]-EFPFD 


EKF 


[34] & [29]-FPFD 


Forward propagation 


L 


L/NL 


N step-lag 




[30,31]-EIFAsyn 


EIF 


[3(),31]-IFAsyn 


Forward propagation 


L/NL 


L/NL 


N step-lag 


Optional recalculation 


[35] 


UKF 


[22] 


Retrodiction 


L 


L/NL 


1 step-lag 




[36] 


EnKE 


[23] 


Linear Interpolation 


L/NL 


L/NL 


N step-lag 




[37] 


PF 


SIR 




L 


L/NL 


N step-lag 




[38,39] 


PF 


SIR 




L 


L/NL 


N step-lag 


MCMC smoothing 


[40] 


PF 


SIR 


Linear Interpolation 


L/NL 


L/NL 


N step-lag 




[41] 


PF 


UPF 


QUKF(x tm \x{^ ,x { t 3 ^ ,z tm ) 


L 


L/NL 


N step-lag 




[42]-A 


PF 


SIR 




L/NL 


L/NL 


N step-lag 




[42]-Bl 


PF 


MPF 


Fixed-point EK Smoother 


L/NL 


L/NL 


N step-lag 


Check diversity 


[42]-B2 


PF 


MPF 


Fixed-point UK Smoother 


L/NL 


L/NL 


N step-lag 


Check diversity 


[42]-B3 


PF 


MPF 


Fixed-point Particle Smoother 


L/NL 


L/NL 


N step-lag 


Check diversity 


[43] 


PF 


MPF 


9-Retro(a= tm |a4^) 


L 


L/NL 


N step-lag 




[44] & [45] 


PF 


[42]-B# 


[42]-B# 


L/NL 


L/NL 


N step-lag 


Check diversity 



An analysis of the table shows that the OOS non-linear algorithms that can be used for systems 
with linear and/or nonlinear systems with measurements delayed more than 1-step lag are: the OOS 
EIF [30,31]-EIFAsyn, the OOS EnKF [36], the OOS SIR [40], and the OOS MPFs [42]-B#, [44] 
and [45]. The use of a linear interpolation operation to obtain the location at the time stamp of a 
delayed measurement in [36] and [40] reduces the applicability of these algorithms to those systems 
with slow dynamics or whose position between two time steps can be approximated by the line that joins 
the positions of the previous and posterior time-steps. The remaining techniques are equally general, 
although the computational overload associated to the OOS EnKF (penalized further in the multisensorial 
case by the use of as many EnKFs as sensors) and OOS MPFs (penalized further by the operations of 
the fix-point smoothers), makes the OOS EIF [30,31]-EIFAsyn the most efficient solution, when there 
are some linear or weakly non-linear measurement models that make the filter avoid the recalculation of 
the sensorial information associated to them. 

2.2. Autonomous Robot Control and Tracking Systems that Deal with the OOSP 

To the best of the authors' knowledge, there is not any published localization module that forms 
part of a control system of an autonomous mobile robots that deals with the OOSP using an OOS 
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algorithm, instead of throwing the delayed measurements (easiest and more frequent approach), delaying 
the operations [11], or re-starting the filter from the time stamp of each arriving measurement [12,13]. 

However, the majority of the OOS non-linear filters analyzed in the previous section are used to track 
the location of mobile objects. Based on the information contained in the robot location x t estimated by 
the OOS non-linear filter, the tracking problems (and the OOS filters) can be organized as: 

1. Pose (p x ,p y ) + Velocity (v x ,v y ) Estimation Problems with a linear dynamic velocity constant 
model. The OOS LKF [26], OOS EKF in [27-29], OOS UKF in [35], OOS EnKF in [36], OOS 
PFs in [37-39,43] have been tested against this type of model. Besides, the OOS EKF [29]-EAl 
and [29]-EFPFD are implemented successfully inside an automative pre-crash system [46]. 

2. Pose (p x ,p y ) + Velocity (v x ,v y ) + Orientation (p 9 ) Estimation Problems, where the dynamic 
model is the non-linear coordinated turning model in [47]. The PFs in [42,44,45] have been tested 

(i) (i) 

against this type of model. The OOS UPF [41], whose q UKF (x tm \x t J , x\ b , z tm ) requires a linear 
transition model, is tested against this type of problem too, because the non-linear transition model 
f(x ti ,u tkjt .) of the coordinated turning model can be factored as F tkyt .(x tk )x tk . Besides, two 
reduced version of this estimation problem, consisting on estimating only the pose (p x ,p y ) and 
orientation (p e ) with a non-linear dynamic model are used to analyze the performance of the OOS 
EIF [30,31]-EIFAsyn and the OOS SIR in [40]. 

From the point of view of a tracking system both problems are equally interesting, although the second 
is more difficult due to (1) the non-linearities in the transition model and to (2) the discontinuity between 
0 and 2n (or between — n and n) associated with the orientation p 6 . From the point of view of a control 
system whose actions depend on the location estimates, the second type of problem is more complete, 
because estimating also the orientation usually allows a finer control of the robot. 

2.3. Suggested OOS Algorithms for the Location Module of an Autonomous Robot Control System 

Based on the previous analysis, when the robot dynamics are modeled with non-linear expressions in 
the localization module of the control system of an autonomous mobile robot, we suggest to select those 
algorithms that have an inherent complete non-linear support ([36,40,42,44,45] and [30,31]-EIFAsyn) 
and/or whose performance has been tested already with non-linear modeled systems ([40-42,44,45] 
and [30,31]-EIFAsyn). Besides, we consider that the OOS EIF [30,31]-EIFAsyn is especially interesting 
in those cases where a low computational load is required in the localization module, because it avoids 
the computational burden associated to the OOS PFs [40-42,44,45] and OOS EnKF [36]. 

When the robot dynamics can be modeled with linear expressions, any of the techniques with linear 
and/or non-linear dynamic model support can be applied. However, special care should be taken with the 
OOS EKFs that by default do not consider that those sensors with strong non-linearities can require to 
have the information recalculated when measurements with older time stamps arrive later than the ones 
associated to them that have been already assimilated. 

3. A Case Study 

This section illustrates the benefits of using one of these OOS techniques by implementing an 
adapted version of [30,31]-EIFAsyn in the localization module of the control software of one of our 
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robots. Additionally, its non-linear dynamics and combination of linear, weakly non-linear and strongly 
non-linear sensors let us show the importance of analyzing the properties of all the sensors to minimize 
the computational requirements while maintaining the performance of the selected filter. Finally, this 
section also includes a performance comparison of the selected technique against a subset of the 
previously recommended filters. 

3. 1. The Robot Dynamic and Sensorial Models 

In this section we present the dynamic and sensorial models associated to the localization module of 
the autonomous mobile robot that we will use in our experiments. Besides, the constants and covariances 
of the models are presented in Section 3.3.1, and its Jacobians in the Appendix. 

The robot, represented in Figure 2, is equipped with two motorized wheels (independently controlled 
by two DC drives and placed, separated at b distance, under the lower robot platform) and two castor 
wheels (placed in the front and back of the same platform). The dynamic behavior of a robot with this 
arrangement, which lets the robot rotate around its Z-axis with an angular speed dependent on the control 
speed applied to each wheel, is going to be modeled as a non-linear system. The sensorial devices of the 
robot used to estimate the robot location are: two encoders attached to the motorized wheels that provide 
information about the displacement of the wheels, a magnetic compass that provides information of 
the robot orientation, and an ultrasonic belt that provides information of the robot distance to known 
landmarks. These three types of sensors are going to be modeled with linear and non-linear models. 
Finally, it is worth noting that the robot is also equipped with an stereoscopic visual system and an 
infrared belt, whose information is only used in the module in charge of updating the map information. 

Figure 2. Robot, (a) Schema; (b) Frontal View; (c) Lateral View. 
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3.1.1. Robot Dynamics 

The location x t of the robot at each time step consists of five components: the robot pose (pf, p\ ), its 
orientation (p e t ), and the linear and angular displacements of the robot (A l t , A 9 ). The control signal u t 
has two components [u^ , uf ) that represent the velocity applied to the Left and Right motorized wheels. 

The dynamics of our robot can be modeled with different types of expressions [48,49]. The selected 
model, whose transition function /(•) is presented in Equation (3), decomposes the robot movement 
in a rotation (Af) followed by a linear displacement (Aj), without considering the thickness of the 
motorized wheels or the rotations that occur while the robot is displacing. In the model, the linear 
displacement (A[ +1 ) is obtained as the mean displacement obtained by both motorized wheels moving 
at speeds (u^, uf) during dt seconds, while the angular displacement (Af +1 ) depends on the difference 
of the displacement of both wheels and the distance (6) that exists between them. Finally, it is important 
to highlight the fact that the predicted orientation (p e t+ i) is obtained incrementally, and therefore its 
values can be out of one of the usual angular range (such as [0, 2n) or [—it, n)). Therefore, in order to 
keep the values in a given range, it is advisable, although not necessary, to put them into range after 
the prediction. 
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The covariance matrix Qt+i,t of the white noise added to the transition function is presented in 
Equation (4), and depends of the square of the elapsed time dt. Note that this Qt+i,t makes the noise 
accumulate directly in the displacement variables and indirectly in the pose and orientation. 
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3.1.2. Compass 



The robot is equipped with an electronic compass that provides information (zi^+i) of the robot 
orientation (p e t+ i). However, due to the discontinuity and periodicity of the angular data, the 
measurement function hi{-) should receive a special treatment, which takes into account if the 
angular discontinuity forms part of the shortest or longest path that exist, according to Figure 3(a), 
between the compass measurement zi^+i and the robot orientation (Pt +1 ). When that happens, and in 
order to make the robot location comparable to the compass measurement, the pf +1 value should be 
incremented/decremented for 2n radians. The measurement function /ii(-) that represents this behavior 
is presented in Equation (5). The first expression is for the case where the discontinuity is in the shortest 
path and Zn+i is v i and p e t+1 is v 2 . The second expression is for the case where the discontinuity is 
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in the shortest path and Zi jt +i is v 2 and p e t+1 is V\. And the last expression is for the case where the 
discontinuity is not in the shortest path. 

{Pt+i - 2?r if z i,t+i e [0, 7r) a |* m+1 - > 7T 
pf +1 + 27T if Zx,t+1 £ 2tt) A |z ljt+ l - > 7T (5) 

otherwise 

The covariance matrix Ri,t+i °f the white noise added to the compass measurement function, 
presented in Equation (6), is the same for every compass measurement. 



Rl )t +1 = [ Var CO mpass ) (6) 



Figure 3. Sensor Models, (a) Orientation; (b) Ultrasonic belt. 




3.1.3. Encoders 



The robot is also equipped with two incremental encoders that can be used to obtain information about 
the displacement of each motorized wheel. This displacement, represented as z^f+i and z^t+i for the 
Left and Right wheel, is linearly related, as the encoder measurement model in Equation (7) captures, to 
the robot linear and angular displacements (A' +1 and A^ +1 ). 

2 ' t+1 ~U% J V A m + A m* 6 / 2 

The covariance matrix i^.t+i of the white noise added to the encoder measurement function, presented 
in Equation (8), depends on the square of dt and considers that the errors of the encoders are independent. 

B^^&J™* ° ) (8) 

3.1.4. Ultrasonic System 

Finally, the robot is also equipped with a belt of 8 ultrasonic sensors numbered from s = 3tos=10 
(as s = 1 and s = 2 are already associated to the compass and the encoders), distributed around the upper 
robot platform as Figure 3(b) shows, able to measure the distance of objects placed between 0.2 and 
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2.0 m. This distance measurements can be used to determine the robot pose (pf + i, Pt+i) when the robot 
is placed on an environment with known walls, corners and columns. In order to establish the relation of 
the robot and landmarks poses, we use the relations proposed in [50], where: 

• The distance between a wall and the sensor can be calculated as the distance between the 
normalized line (A obj x + B obj y + C ob j = 0, with A 2 ob - + B 2 bj = 1) that defines where the wall 
is placed and the sensor pose (x, y), as far as the orientation of the sensor and the wall fulfill the 
constraints imposed by the sonar directivity. 

• The distance between a corner and the sensor can be calculated as the distance between the corner 
pose (Po b j,p y ob j) and the sensor pose (x, y), as far as the orientation of the sensor and of the line that 
joins the sensor with the corner fulfill the constraints imposed by the sonar directivity. 

• The distance between a column and the sensor can be calculated as the distance between the circle 
of radius (r ob j) and center {p x ob j,p y ob j) that represents the column, and the sensor pose (x, y), as far 
as the orientation of the sensor and of the line that joins the sensor with the center of the column 
fulfill the constraints imposed by the sonar directivity. 

In order to relate the robot pose (p x +1 ,p y +1 ) directly with the distance provided by the sensors, we 
can use the relation between the robot pose and sensor disposition (represented by the distance d s and 
orientation a s according to Figure 3(b)): 

x = p x +1 + d s cos{p e t+1 + a s ) ^ 
y = P v t+ i + d s sin(pf +1 + a s ) 

Taking into account all these factors, the non-linear function that models the behavior of each ultrasonic 
sensor (s 6 [3 : 10]) with respect to a map of given landmarks are represented in Equation (10), where 
obj represent the detected object and its type. In order determine which object has been detected, we 
run the model, taking into account the sonar orientation and directivity for all the objects of the map and 
select among all the possible identified objects that are not occluded by others the one that will produce a 
shorter distance signal, because it is the one that has a higher chance of having reflected the sonar signal. 



\A obj [p% +1 +d s cos{pf +1 +a s )] +B obj [p y t+l +d s sin(p 9 t+1 +a s )] +C* 0 , 
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r ob 3 



obj = wall 

obj = corner qq\ 
obj = column 



The covariance matrix R S) t+i with s E [3 : 10] of the white noise added to the ultrasonic 
distance measurement function, presented in Equation (11), is the same for every sonar and 
distance measurement. 

R s ,t+1 = (var sonar ) (11) 



3.1.5. Models Summary 



In short, the expressions used to model the robot behavior have the following characteristics: 
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• The dynamic/transition model is non-linear. Besides, the angular location has a limited 
and periodic representation. The influence of this fact in the correct behavior of the filters will be 
analyzed in the following sections. 

• The compass model is also non-linear due to the ±2n correction term that appears, dependent on 
the value of z^t+i and p 6 t+1 , in certain cases as a consequence of the discontinuity and periodicity 
of the angular data. 

• The two encoders grouped together in z 2 t +i are modeled using a unique linear expression and 
covariance matrix. 

• The behavior of the sensor that combines the measurement of each ultrasonic sensor s 6 [3 : 10] 
and a map of objects is represented with a non-linear model. 

That is, we are going to estimate the location of a robot using a non-linear transition model, and linear 
and non-linear measurement models. 

3.2. EIFAsyn: The Selected Estimator for the Localization Module 

In this section, we present an adapted version of EIFAsyn, the OOS EIF that consists of sets of 
EKF predictions, EIF updates and projections between the state and information space [30,31], that has 
been slightly modified in order to (1) consider the peculiarities of the angular data and to (2) include 
a validation step to decide before assimilating the sensorial information, whether the measurement is 
corrupted or not. 

The two main steps of this version of EIFAsyn, prediction (carry out to make the filter estimate the 
state of the next time step) and measurement update (performed when any measurement is arrived), 
are presented in Figure 4, where £ s ,k,t represents, maintaining the nomenclature used in [30,31], the 
measurement z Sj k taken by sensor s at time k that arrives at the localization module at t. Besides, 
x k \ k and P k \k stand for the mean and covariance of the location estimated with all the measurements 
that have been assimilated so far, yk\k and Y k \u for the information of the estimated location and its 
covariance, i& and I k \ k for the accumulated sensorial information and accumulated sensorial information 
covariance of the sensors whose information does not need to be recalculated, and i r k ec and I r k ^ for 
the accumulated sensorial information and accumulated sensorial information covariance of the sensors 
whose information has to be recalculated. Finally, the additional operations that do not appear in the 
original version of EIFAsyn are presented in red to make them easily identifiable. 

Figure 4. Prediction and Measurement Update Steps of the adapted EIFAsyn. (a) Prediction 
step from t — 1 to t; (b) Update step for $, s ,k,t (measurement of sensor s with time stamp k 
arriving at t). 

F t ,t-i = (J f) x (x t -i\t-i,ut,t-i,t,t - 1); //Jacobians 

&t\t-i = f{&t-i\t-i,ut,t-i,t,t- 1); P t \t-i = Ft,t-iPt-\\t-iF?,t--L +Qt,t-i; //(P) 
CorrectAngle(Xj +1 \j) //Angle in [0,2ir) 

vt\t-i = P t Jt-i&t\t-i; Y t \t-i = Pt\t-x // ( ± ^) 

% t = 0; I t = 0; i\ ec — 0; I\ ec = 0; //Sensorial information initialization 

(a) 



Sensors 2012, 12 



2500 



Figure 4. Cont. 



x k \k-\ - 5 /r fe | fe _ 1 yft|ft-i; Pk\k-x - ^feife-ii (J-s) 

J? S! fe = (Jh s ) x (x k \ k _ 1 ,k) //Jacobian 

e s ,fc = £ s ,fc,t - /i s (x fc | fe _i,fc); d s , fc = e T sk (H St kPk\k-iH^k) e Sjfc ; //Mahalanobis distance 
i/ d s>k < l s ,k //If £s,fc,t is valid 

C^k.i = &>,M + H s,kX k \k-i ~ h s {x k \ k ^ x ,k) //Corrected measurement 

i a ,k = H J,k R 7,k€?,k,t'' I s,k = H J,k R 7,k H s,k] //(-Lm) 

z/ (s ^ rec) 

= ifc + is.fe; -ffe = Ife + I s ,fc //(Al) 

e/se 

l*fc eC = ir k C + isX, I k ec = I r k ec + IsX, store £ SjM ; //(A WL ) + storage 
end 

/or j = k : t ////Update+prediction loop 

feu = %b--i + <j + *r c ; = Y ]b -i + h + ir a {A- all) 
*/ i < t 

f j+ij = { J f)x{xj\j,u j+Xd ,j + 1, j) //Jacobian 

Corr - ectAn5 , ie(xj4_iu) //Angle in [0,27r) 

yj+i|i = Jj+^i+itf » = P jM-iy /'(-Li) 

= 0; , JJ£° = 0 //Non-linear Sensorial information initialization 
for s € rec //Recalculating sensorial information of some sensors 
H a,j+i = {Jh„) x (xj + i\j,j + 1) //Jacobian 

£s,j+i,a = £s,j+i,a + H s<j+1 x j+ i\j - h s {x j+ i\j,j + 1) //Corrected measurement 

ij+i = ij+i + Hj,j+iR7,j+i€?,j+i,a'i f J+i = r j+i + H I,j+i R 7j+i H s,j+u //(-Lm) + (A NL ) 
end 
end 
end 
end 

(b) 

The EIFAsyn prediction step (Figure 4(a)) is the usual prediction step of the EKF (Jacobians + (P)), 
followed by the additional operation that corrects the angular data, a projection of the location and its 
covariance into to information space (_Lj), and an initialization of the sensorial information variables. 

The EIFAsyn update step (Figure 4(b)) consists of several steps. It starts with a validation step 
that calculates the Mahalanobis distance d s ^ (which weights the discrepancy between the measurement 
and predicted measurement with the inverse of the predicted measurement covariance) and checks if 
d s> k is under a threshold l s selected to ensure that the test only rejects valid measurements with a 
selected probability [15,5 1]. To assimilate the valid measurements, the update step projects the corrected 
measurement t into the information space {-Lm), accumulates their values to the remaining sensorial 
information associated to the same time step (A L or A NL ), and optionally stores the measurement. 
Afterwards, it starts an assimilation-prediction loop, where all the sensorial information at each time 
step is accumulated into the predicted information values {A all), projected into the state space (J_s), 
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predicted (P), corrected, and projected again into the information space (_Lj). Additionally, the algorithm 
recalculates the sensorial information of those sensors that require this operation (J_m + Awl)- 

The recalculation of the information of sensor s is required when H s j +1 Xj + iy — h s (xj + i\j,j + 1), 
the term that corrects the measurements £ s j_|_i a , changes its value significantly with the changes that 
occur to Xj+i\j during the assimilation-prediction loop. For the linear sensors, the recalculation is never 
necessary because H s j +1 x j+1 y — h s (x j+ iy,j + 1) = 0. Moreover, in that case £f fc t = $, s ,k,t- For the 
non-linear sensors, the recalculation is unnecessary when the changes are negligible and it is required 
when the changes are abrupt. 

Therefore it is necessary to study the behavior of the non-linear sensors before deciding to exclude 
them from the recalculation set. In our problem, we have to analyze the behavior of the: 

• Compass: Given a compass measurement Zij+i, the value returned by the compass model 
(Equation (5)) changes abruptly depending on the position of the indetermination (0, 2n) with 
respect to the shortest path between Zij + i and the estimated angle Pj +X , while the value of 
Hij + iXj + i\j is always p e j+1 as Hij+i (Equation (13)) remains unchanged. Therefore, when 
a new delayed measurement modifies the estimated value of placing it at the opposite 
side of the indetermination it was before the assimilation of the new measurement, there can 
be an abrupt change of ±2n between the previous-assimilation and post-assimilation values of 
Hij +1 Xj + i\j — hi(xj + xu,j + 1). As when the robot orientation is close to the indetermination, 
delayed measurements can easily change the values of p® +1 to the opposite site, the already 
assimilated compass information needs to be recalculated when any delayed measurement arrives 
to avoid the erroneous operation of EIFAsyn. 

• Ultrasonic system: Given a sonar measurement z s j + i with s G [3,10], the value returned 
by the ultrasonic model (Equation (10)) and the value of H s j +1 Xj + iy (Equation (13)) are 
smoothly changed with small modifications of the estimated robot pose Therefore, 
when the robot pose estimate is slightly modified by the arrival of a given measurement, the 
previous-assimilation and post-assimilation values of H s j +1 Xj +1 \j—h s (xj +1 y,j + l) do not change 
significantly. Therefore, we can consider the exclusion of this sensor from the recalculation set. 

The validity of both choices has been tested in multiple simulations. Before presenting their results 
in the following section, it is worth noting that the necessity of recalculating the sensorial information 
associated to some non-linear sensors is a requirement of EIFAsyn and of the OOS EKF (which can not 
be used for our problem due to the additional requirement of the linearity in the transition model), due 
to the direct relationship that exists between the EKF and EIF [3]. Besides, in the cases where the robot 
control software can support the computing overload of the OOS PF (that will not need to recalculate 
information associated to the already assimilated measurements) or OOS EnKF, those algorithms should 
also take into account the existence of the indetermination in the angular data when calculating the mean 
and covariance values of the robot locations based on the values of the particles or ensembles. 

Finally, note that the additional step associated to the correction of the angle does not change the 
behavior of the algorithm significantly. Moreover, it is only a convenient modification. However, the 
step associated to the validation step, which is useful to reject those measurements that are not valid 
due to the malfunction of the sensors or corrupted during the communication through the network, has 
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a bigger impact on the results of the algorithm. Moreover, as the validity of the measurements is only 
tested once using the location estimate in the test, the validity of the measurements that are in the limit 
can depend on the order of arrival of the measurements. However, as the analysis of the validation for the 
asynchronous version of IFAsyn shows [15], the influence of the order of arrival can be minimal when 
the filter is well-tuned. 

3.3. Results Obtained with EIFAsyn 

In this section we present the results obtained by EIFAsyn for the localization of our autonomous 
mobile robot with simulated and real data. The simulated experiments are set up to see which of the 
non-linear sensors require the recalculation of its measurements information when there are delayed 
data and what happens if the OOS data is not used. The real experiment shows if the localization module 
based on the OOS-EIF works properly when it is part of the control software of the actual robot. 

Figure 5(a) shows the setup of both experiments. The red dots represent all the known corners while 
the blue lines represent all the known walls. In the simulated experiment, the robot is placed in the 
initial position (o), oriented towards the final position (*), and controlled by applying equal speeds to its 
motorized wheels in order to move it around the angular indetermination [0, 2tx}. Therefore, it follows 
the blue trajectory presented in Figure 5(b). In the real experiment, the robot is placed in the initial 
position (o) and required to go to the final position (*), unknowing that there are the square and round 
objects (marked in green in Figure 5(a)) in the hall. The control software of the robot where the OOS 
algorithm is embedded [52] makes the robot initially go towards the final position until it locates the 
unknown objects, updates the occupancy grid [53], and replans new trajectories to avoid the unknown 
objects locations. Therefore, the robot follows the red trajectory presented in Figure 5(c). 

Figure 5. Experiments Setup, (a) Map Objects; (b) Simulated Experiment; (c) Real 
Experiment. 




3.3.1. Simulations 

In order to analyze what happens if the delayed measurements are not used and which non-linear 
sensors require the recalculation of its information when there are delayed measurements, we run 
multiple simulations to generate all the sensors measurements every 0.1 s and run EIFAsyn, for 
the same generated data, considering 5 different scenarios: (1) using all the measurements without 
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delay; (2) using all the measurements with delays in the compass data without recalculating the sonar 
information; (3) using all the measurements with delays in the sonar data recalculating the compass 
information; (4) using all the measurements with delays in the sonar data without recalculating the 
compass information; and (5) not using the delayed measurements of the compass and sonars. We 
use constant delays of 1 s in all the cases, i.e., of 10 time-steps, in order to increment the challenge 
of the validation step (which only considers the already arrived measurements) of the adapted version 
of EIFAsyn presented in Section 3.2. The lack of delays in the first case makes EIFAsyn work as the 
EKF and therefore, the behavior of the filter in this scenario is considered the reference to be achieved 
by EIFAsyn in the remaining scenarios. The delays of the compass measurements in the second are 
used to see that the sonar information, arrived before the compass one in this case, does not have to be 
recalculated. Similarly, the delays of the sonar measurements in the third and fourth are used to see if the 
compass information, arrived before the sonar one in these scenarios, has to be recalculated or not. In 
other words, scenarios 2, 3, and 4 let us check if the non-linearities of the sonar and compass are soft or 
hard. Finally, the fifth case justifies the necessity of the OOS algorithm when all the compass and sonar 
information arrives delayed, as it happens in the real robot experiments. 

As the non-linearity of the compass is due to the angular indetermination [0, 2tt], we set up the 
simulated experiments to make it move around it, placing the robot initially oriented towards 0 rad, 
and applying the same control signals (10 cm/s) to both motorized wheels during the whole simulation. 
The simulated data is obtained with the same models that are incorporated in EIFAsyn, although as 
Table 2 shows the variances used to obtain the simulated data (fourth row) are lower than those used in 
EIFAsyn (and in the other filters under test in Section 3.4, fifth row). The difference in the variances is 
due to the following factors. On one hand, we originally set up the variance values in the simulations 
accordingly to the values of the transition and measurement noise levels that we have observed in our 
robot (third row). However, we have to significantly reduce the simulation value of var^e, because the 
bigger real value significantly affects the simulated robot orientation and we want it to move with two 
equal fixed control signals (i.e., without a feedback control loop) around the angular indetermination 
without diverging significantly from it. On the other, the variance values in EIFAsyn are bigger than the 
observed in order to reduce the effects of the non-linearities in the suboptimal EKF without delays. 



Table 2. Constants and Variances of the Models. 



Variable 


dt 


b 


Sonar Directivity 


var^i 


var^e 


VCLV corn p ass 


varjv L 


var N[i 


VCbT 'sonar 


Dimensions 


(s) 


(cm) 


(rad) 


(cm/s) 2 


(rad/s) 2 


(rad) 2 


(cm/s) 2 


(cm/s) 2 


(cm) 2 


Real Robot Data 








(I) 2 


(2tt/180) 2 


(2V180) 2 


(0.5) 2 


(0.5) 2 


(1.5) 2 


Simulation Data 


0.1 


24.5 


12tt/180 


(I) 2 


(0.25tt/180) 2 


(2V180) 2 


(0.5) 2 


(0.5) 2 


(1.5) 2 


EIFAsyn Data 








(2) 2 


(3tt/180) 2 


(3V180) 2 


(I) 2 


(I) 2 


(2) 2 



The results of EIFAsyn for the different setups and the values obtained during one of the simulations 
are summarized in the graphics presented in Figure 6. 
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Figure 6. Simulated Experiments. 
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The first column shows the behavior of the sensors and validation tests representing all the 
measurements received by EIFAsyn grouped by sensor (color) and status: V- non-delayed and valid, 
NV- non-delayed and non- valid, L&NV- delayed and non- valid, and L&V- delayed and valid. The 
second column shows the estimated value of the location (p x ,p y ) and orientation (p g ), and the third 
represents their covariances (a xx , a yy , age). In each row, we group the results of the same scenario. The 
estimated location (p x , p y ) in the five cases is also presented in Figure 5(b) (the trajectories of the second 
and third case are occluded by the trajectory of the first, while the trajectories of the fourth and fifth 
scenario diverge). The graphics in the first column show that as a consequence of the robot orientation 
in the experiment setup S8 and S9 are the only sonars providing measurements. Moreover, S8 only 
gets a few measurements associated to the corners in the bottom U-shape of the hall, which provide 
both information associated to p x and p y and are responsible of the decrements in covariance of the 
p x state. The continuous stream of measurements of S9 are associated to the bottom walls of the hall, 
provide information about p y and are responsible of the the small covariance of the p y state. Finally, the 
continuous stream of measurements of the compass provides information about p e and is responsible of 
the the small covariance of the pg state. 

At a first glance, the state and covariance graphics of the first, second and third row look really 
similar. That is, EIFAsyn obtains similar results when the data is non-delayed, when the compass data 
is delayed and the filter does not recalculate the non-delayed sonar information, and when the sonar 
data is delayed and the filter recalculates the non-delayed compass information. The sensor behavioral 
graphics look different because they encode both the existence of the delays and the results of the 
validation. If we compare only their validation status, we can observe that the same data is validated in 
the experiment without delayed data (first row) and with delayed sonar measurements recalculating the 
compass information (third row), making the state and covariances under both setups equal. However, 
the validation status of the experiment with delayed compass data (second row) and without delayed 
measurement (first row) is slightly different, because the order of arrival of the information can slightly 
affect the validation step [15]. This discrepancy justifies the slightly differences that also appear in the 
a xx covariance graphics of the first and second row, because rejecting a piece of information from the 
sonar S8 visually affects the values of a xx as the sensorial information directly associated to the p x state 
is really sparse. 

The results presented in the fourth row of Figure 6 are significantly different to the ones shown in the 
previous rows. The discrepancy starts after t = 46 s, where it happens to be a change of the value in pg 
in all the scenarios around the indetermination that should be taken into account in order to re-assimilate 
the non-delayed compass information when the delayed sonar information becomes available. However, 
as EIFAsyn is not required to recalculate the compass information in the fourth case, the arrival of the 
delayed sonar information makes the already assimilated compass information be in the wrong side of the 
indetermination and the estimate of the orientation pg incorrect (0.8 rad ~ 45°). At that moment, EIFAsyn 
loses track, because the wrong estimated orientation makes it reject the compass and sonar measurements 
afterwards and wrongly update the robot location and orientation, as well as its covariances, using only 
the encoder measurements. 

When we generate the simulated data 100 times with the same setup, we observe the same type 
of erroneous results, sooner or later, 46 times. That is, while moving around the indetermination, the 
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compass information requires to be recalculated almost in half of the simulations. The discrepancies 
associated to the non-recalculation of the sonar information are minimal and negligible in all the tests. 
Therefore, and in order to avoid the loss of track of the filter when it is embedded in the control 
architecture of a real robot, we treat the compass and sonar respectively as sensors with strong and 
soft non-linearities, i.e., only recalculating the compass information. 

Finally, the results presented in the fifth row of Figure 6 are different too, because using only the 
information provide by the encoders, directly related to the linear and angular displacements, makes 
EIFAsyn increment continuously the covariances of the robot location and orientation (a xx , a yy , aee) and 
have a poor estimate of the robot location (p x , p y , p e ). Therefore, for this problem it is really important 
to be able to use the OOS measurements of the compass and sonars efficiently in order to let the robot 
control software navigate successfully, as the experiment in the following section shows. 

3.3.2. Real Experiments 

In order to illustrate the behavior of EIFAsyn within the localization module of the control software 
of a real robot, we place the robot in the same hall with the same known objects and two unknown ones, 
and let the control software generate the control signals (within the range [0,15] cm/s) based on the 
robot location estimated by EIFAsyn and the obstacles locations estimated using the occupancy grid and 
Bayesian filter in [53]. 

The results of the experiment are presented in the sensorial behavior, state and covariance graphics 
of Figure 7. The sensorial behavior graphics (Figure 7(a), 7(b) and 7(c) show that all the measurements, 
except those provided by the encoders, arrive delayed and out of sequence at the location module. 
Besides, although the compass and encoder measurements are usually valid, the sonar ones are usually 
rejected by EIFAsyn, because they are either (1) associated to the unknown objects of the map and can 
not be used by EIFAsyn or because (2) they are rejected by the EIFAsyn validation test as their error, 
originated for instance from bounces of the sonar signals in the multiple walls of the bottom U- shape of 
the hall, is not included in the models. Within the unknown object group fall all the sonar measurements 
outside the cyan squares in the sensorial behavior graphics, i.e., all the measurements by sonar S5 and S4 
(due to the round object), all by S8 (due to the square object), the two first by S7 (due to the square and 
round object respectively), the first group by S3 (due to the square object), the one at t=60 s by S9 (due 
to the square object), and the ones by S6 up to t=80 s (due the first group to the square and the second to 
the round one). 

The sparsity of the sonar information makes the OOS filter of the localization module of the control 
software estimate the robot localization using almost all the compass and encoders information, and 
only a few measurements provided by the sonars. The small amount of (p x ,p y ) information makes 
the covariances of those variables (<J xx ,a yy ) grow, while the almost continuous stream of compass 
measurements makes the angular variance (age) really small (see Figure 7(e)). In spite of these facts, 
EIFAsyn is able to locate properly the robot, as the discrepancy between the real and estimated final 
robot position falls within the covariance values. Additionally, in this experiment, we can observe how 
EIFAsyn can need to recalculate the compass information when the measurements of the sonars arrive 
after the compass ones, due to the abrupt changes of the pe values that appear in Figure 7(d) around the 
angular inde termination. Besides, the control software is able to update the occupancy grid using the 
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robot position estimates and all the sonar measurements. Finally, the reduced computational overload 
associated to EIFAsyn is negligible inside the control software architecture, which consists of multiple 
modules of planning and control levels. 

Figure 7. Real Experiments, (a) Sensor behavior; (b) Sensor behavior; (c) Sensor behavior; 
(d) State; (d) Covariances. 
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3.4. Comparison of EIFAsyn with Other General OOS Approaches 

In this section we compare, using the robotic problem study in this paper, the behavior of EIFAsyn 
against the performance and efficiency of some of the general N-step lag OOS algorithms suggested in 
Section 2.3. The non-linearity in the dynamic model of the robot reduces the possible comparison to the 
OOS EnKF in [36] and the OOS PFs in [40-42,44,45]. Among all these possibilities, we have decided 
to analyze the performance of the OOS EnKF in [36] and the OOS PF in [42]-A using the models and 
simulated data presented in Sections 3.1 and 3.3. With this selection, EIFAsyn is compared against two 
filtering techniques (EnKF and PF) that differ from the EIFAsyn EKF/EIF support. Besides, our problem 
tests the OOS EnKF in a more challenging situation that the one presented in [36], because our robot has 
a non-linear dynamic model with multiple sensors providing information related with different states of 
the system, and the system in [36] has a linear dynamic model and two equal sensors. Finally, the OOS 
PF [42] -A (1) equals the OOS PF in [40] when there is a non-delayed measurement (in our case provided 
by the encoders) for all the measurement time stamps and (2) the selected PF is computationally more 
efficient than the OOS PFs in [42] -B# and [41,44,45]. 
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In order to carry out the comparison, we have disabled the validation step of EIFAsyn because 
the other algorithms do not include it and we want all the filters to be able to use the same 
information. Note that the same validation step can not be included in the other filters because it is 
not straightforward to obtain in those filters the predicted mean and covariance values used to validate 
the new measurements in EIFAsyn. Additionally, we compare EIFAsyn with two versions of each of the 
selected algorithms, because: 

• The OOS PF [42] -A estimates the trajectory instead of the state of the system, and therefore, the 
mean and covariance of the state at any time step £j are updated with the already assimilated past 
(tk < U), current (t k = U) and future (U < t k < ti) measurements €s,t k ,t t , while the EIFAsyn 
mean and covariance only include the already assimilated past and current measurements . In other 
words, the standard OOS PF [42]-A mean and covariance values represent the p(x ti \£i;s,t 0 a > u t 0 -.ti) 
while the EIFAsyn mean and covariance values represent p(x ti \£i:S,t 0:i , u t 0 -.u)- That is, the standard 
OOS PF [42]-A mean and covariance values are smoothed with future information that the EIFAsyn 
values does not use. The modified version of OOS PF [42]-A avoids the inherent smoothing 
behavior of the standard OOS PF [42]-A calculating the mean and covariance values without the 
future measurements. Therefore, the results obtained by the modified version of OOS PF [42]-A 
are supposed to be closer to the EIFAsy ones. 

• The OOS EnKF [36] runs an EnKF for each sensor and combines the ensembles of each sensor in 
a unique multisensor fusion ensemble that is used to obtain the mean and covariance of the system. 
In the problem presented in this paper, this way of proceeding makes the standard OOS EnKF [36] 
run 10 EnKFs (the first for the compass, the second for the encoders, and the remaining eight for 
all the ultrasonic sensors) and combine the information of many ensembles (in particular those 
associated to the sonars that are only seeing unrecognizable objects) that have not assimilated any 
measurement. In other words, as many of the sonar EnKFs are working without measurements, 
their ensembles contribute only predicted values to the multisensor fusion ensemble. To avoid the 
influence of these ensembles in the estimated value of the robot localization, the modified version 
of OOS EnKF [36] only combines the results of those ensembles that have updated their state using 
some measurements. 

The two variants of the OOS PF [42] -A and the two variants of OOS EnKF [36] are compared against 
EIFAsyn using the setup of the simulated experiment and the first, second, third and fifth scenario 
presented in Section 3.3. The fourth scenario is not considered in this comparison, because it has 
delayed sonar measurements as the third and it is only required in the EIFAsyn analysis to show that 
this filter has to recalculate the compass information. Besides, the OOS PFs use 2,000 particles to have 
often more than 100 effective particles after assimilating each measurement because this number of 
particles lets us avoid the resampling step after assimilating each measurement, and therefore, it reduces 
the particles depletion (repeatability of the values of the particles in old time stamps) and increments 
the effects associated to the weight update produced by delayed measurements. Additionally, the OOS 
EnKFs use 500 particles in each of the 10 sensor ensembles. That is, we only double the number of 
particles used in each ensamble in [36], with the hope of solving the performance problems presented 
in Section 3.4.1, because with that big number of particles the OOS EnKF [36] variants are already the 
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most computationally and memory inefficient filters as we will show next. In the following sections we 
compare the performance, computational load and memory requirements of the 5 filter implementations. 

3.4.1. Performance Comparison 

Figure 8 shows representative results of the 5 filters, using each row for an scenario, each column for 
a different type of information (the first column represents each filter mean pose, the second the mean 
values of the robot location, and the third their variances), and a different color for each of the filters 
(magenta for EIFAsyn, green for the standard version of OOS PF [42] -A, cyan for its modified variant, 
navy blue for the standard version of OOS EnKF [36], and red for its modified variant). 

On one hand, Figure 8(a), 8(b), 8(d), 8(e), 8(g) and 8(h) show the similar values obtained for the 
estimated locations (the 2ix variations in p e have to be considered negligible too as they occur around 
the 0-2/T indetermination) by the PFs variants and EIFAsyn in the first, second, and third scenario, 
while Figure 8(c), 8(f) and 8(i) show that the PF variants obtain lower variance values than EIFAsyn. 
Therefore, the three filters obtains similar values for our robot location when the compass and sonar 
measurements are or are not delayed, with a tighter covariance in the PF cases. As these behavior is 
observed through different executions of the PFs with the same scenarios and measurements, we can 
conclude that EIFAsyn and the two PFs variants seem equally valid for our problem. 

On the other, Figure 8(a), 8(b), 8(d), 8(e), 8(g) and 8(h) also show how the mean location obtained by 
any of the EnKF variants sometimes (first and third row) significantly differ from the ones obtained 
by EIFAsyn (or the PF variants). As a matter of fact, although the EnKF results selected for the 
second scenario look better than the ones selected for the first and third scenario, the estimated location 
obtained by other executions of any of the two versions of EnKFs in any of these three scenarios can 
significantly differ from the ones obtained by EIFAsyn. In other words, none of the EnKF versions works 
systematically properly with our robotic problem, and the wrong behavior is independent of the scenario 
delays. A detailed analysis of the estimation process makes us believe that this can be due to the fact 
that each of the 10 ensembles is corrected independently with measurements that provide information of 
the different states, and therefore their results can easily diverge due to the lack of the complementary 
information provided by other sensors to other ensembles. The problem seems to be aggravated by 
the sonar ensembles that incorporate only sporadic measurements that lack orientation information. It 
is worth highlighting too that the problem presented in [36] to analyze the behavior of the standard 
version of OOS EnKF is different to ours because (1) it is characterized by a linear dynamic model 
without angular information and (2) its two non-linear sensors are equal and provide a periodic stream 
of delayed measurements. Besides, note that although the results of the standard version of the EnKF 
seem better in some cases, this can be due to the fact that combining the information of all the ensembles 
that work without measurements can minimize the divergence effects of the sonar ensembles that are 
working with sporadic data. Therefore, we can conclude that both versions of EnKF seem invalid for 
our problem, even in the case when the measurements are not delayed. In spite of this invalidity, we will 
also compare against others its computational cost and memory requirements to facilitate the selection 
process of other researchers. 
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Figure 8. OOS Approaches Comparison. 



t3 -i. 
o >. 

Z n 





— EIFAsyn 




PF (STD) 


p 


PF (MOD) 




— EnKF(STD) 




— EnKF (MOD) 






5 


10 15 



P x (m) 

(a) 




20 40 60 
Time (sec) 



(b) 



c 
E 

"O 

2 0 

CD 



20 40 60 



20 



40 



(c) 



60 



20 40 60 
Time (sec) 




— EIFAsyn 
PF (STD) 
PF (MOD) 

— EnKF (STD) 

— EnKF (MOD) 



10 
P.(m) 



15 



(d) 




3. 300 
aT" 280 
( 

=5- 6 
co 4 

a. 0 



20 40 60 



(e) 



20 40 60 
Time (sec) 



c 

E 

o 

> 

CM 
"D 

2 0. 

CD 

D 



■ ' ■■ ■ 

I -\ 




0 20 


40 60 


: 








0 20 40 60 







20 40 60 
Time (sec) 



(f) 




— EIFAsyn 
PF (STD) 
PF (MOD) 

— EnKF (STD) 

— EnKF (MOD) 



10 

P v (m) 



(g) 



15 




20 40 60 
Time (sec) 



(h) 



0 
c\P~ 

E 

■o 

2 0. 

a 
a 
D 



20 



20 



40 



40 



(i) 



60 



60 



20 40 60 
Time (sec) 




— EIFAsyn 
PF (STD) 
PF (MOD) 

— EnKF (STD) 

— EnKF (MOD) 



10 

P v (m) 



15 



(j) 



E 1000 



0 


20 


40 


60 




0 


20 


40 


60 















20 40 60 
Time (sec) 



(k) 




to 0.04^ 
— 0.02P 

CD ^— 



20 40 60 
Time (sec) 



(1) 



Finally, the performance comparison of the filters in the fifth scenario (fourth row) show that for the 
robotic problem we need to incorporate the compass and sonar data, because when any of the filters 
work only with the encoders measurements, the location mean values diverge and the location variances 
significantly grow. 
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3.4.2. Computational Comparison 

In order to measure the computational load of the 5 filters, we measure for the 4 scenarios presented in 
the previous section the time required by each of the filters, which are implemented in C++, when they 
are run over a Intel Core2 Duo at 2.53 GHz with Windows XP. This time is measured in milliseconds 
with the help of the C++ function GetTickCounts(), which retrieves the number of milliseconds that 
have elapsed since the PC was started. The resolution of the function, and therefore of our measurements, 
are in the range of 10 to 18 milliseconds. 

The obtained results are presented in Table 3, directly as mean times (columns from 2 to 6) or as ratio 
time values (columns 7 to 11) with respect the mean time obtained by EIFAsyn for the the same scenario. 

Table 3. Computational Cost Comparison. 



Scenarios 


Mean Measured Time (ms) 


Normalized Mean Time 


EIFAsyn 


PF (STD) 


PF (MOD) 


EnKF (STD) 


EnKF (MOD) 


EIFAsyn 


PF (STD) 


PF (MOD) 


EnKF (STD) 


EnKF (MOD) 


Scenario 1 


1,880 


241,010 


230,840 


602,340 


404,860 


1 


128 


123 


320 


215 


Scenario 2 


2,010 


291,280 


283,670 


618,840 


419,090 


1 


150 


141 


307 


208 


Scenario 3 


2,200 


294,480 


284,940 


618,360 


415,080 


1 


134 


130 


281 


189 


Scenario 5 


320 


181,060 


170,500 


388,950 


242,810 


1 


565 


533 


1,215 


759 



The mean time columns show that for each filter, the computational load of the last scenario (where 
the filters are only predicting and assimilating non-delayed encoder measurements) is smaller than the 
computational load of the first (where the filters assimilate all the available measurements without 
delays) and that the first and fifth are lower than the second and third (where the filters assimilate 
delayed measurements). The similar values within the second and third scenario in all the filters show 
that each filter is similarly computationally penalized when the delays occur in the compass or in the 
sonar measurements. 

The normalized mean time columns show that for the same scenario EIFAsyn has a significantly lower 
computational load than the other filters. Besides, the computational load of the modified versions of 
the selected filters are lower than the computational load of their standard counterparts. In the PF case, 
the bigger computational load of the standard version is due to the necessity of computing the mean and 
covariance values of the intermediate states when any delayed measurements arrives. In the EnKF case, 
the bigger computational load of the standard version is due to the computational difference of fusing 
the information of 10 or 4 ensembles. 

In short, the filters can be sorted according to the computational time, from the best to the worst 
as: EIFAsyn, PF (MOD), PF (STD), EnKF (MOD), EnKF (STD). Finally, it is worth noting that the 
linear relationship that exists in the selected PF and EnKF between the number of particles and the 
computational load of the algorithms will make these PFs/EnKFs with only 200/50 particles (which are 
are not enough to make any of the filters work properly) still have a computational load one order of 
magnitude bigger than EIFAsyn. Therefore, the low computational load of the EIFAsyn, in spite of its 
necessity to recalculate the information associated to the sensors with strong non-linearities, is a plus 
that can not be underestimated. 
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3.4.3. Memory Comparison 

In order to compare the memory requirements of the 5 filters, we theoretical count the number of floats 
that are necessary to store the minimal information required to make them assimilate any measurement 
that is delayed at maximum W time steps. 

The results of this count are presented in the second column of Table 4, where N x stands for the 
number of states of the system, N s for the number of measurement variables returned by each of the s 
sensors whose information needs to be recalculated, N p for the number of particles in the algorithm, and 
S for the number of sensors in the system. The EIFAsyn formula is counting, for each time step in W, 
the space associated to 2 state variables + 2 covariance variables + the measurement variables whose 
information has to to be recalculated. The PFs equation is counting the memory required for saving 
the total number of particles in the PF in each time step + the weights associated to each PF trajectory. 
Finally, the EnKF expression is counting the required memory to store the number of particles/elements 
in each of the S ensembles during the W time steps. 

Table 4. Memory Comparison. 



Filter Theoretical Float Count Applied Float Count (W = 10) 



EIFAsyn W ■ 2 • (N x + N%) + ^ AU 620 

\ s£rec / 

PF (STD & MOD) W ■ N p - N x + N p 102, 000 

EnKF (STD & MOD) W ■ S ■ N p ■ N x 250, 000 



The results of the count for our problem (N x = 5, N s= i — 1, S — 10) and filter configurations 
(N p = 2000 for the PFs and N p = 500 for the EnKFs) are presented in the third column of the table. The 
worst filter regarding the memory requirements is clearly EnKF, followed closely by the PF. Besides, 
EIFAsyn memory requirements is two orders of magnitude smaller than the others. Therefore, regarding 
the memory cost, EIFAsyn is also the best option for the problem. 

3.4.4. Overall Comparison 

Taking into account the performance comparison, we conclude that while the OOS EnKF variants 
are invalid for our problem, both the OOS PF variants and EIFAsyn are similarly applicable. Besides, 
between the two valid possibilities, the memory and computational requirements of EIFAsyn are 
significantly smaller, making it an ideal choice to be incorporated in the complex control architecture 
of a robotic system like ours. 

4. Conclusions 

This paper presents a thoughtful analysis of the different possibilities of OOS algorithms that exist for 
estimating the location of non-linearly modeled robots, taking into account the properties of the existing 
algorithms and of the robotic problem. Therefore, this analysis can be used to determine which are 
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the best algorithms for different types of non-linearly modeled OOS estimation problems, as well as to 
know which problems similar to the autonomous mobile robot location estimation one have already been 
tackled with which algorithms. 

Besides, this paper also shows how one of the most generic and efficient OOS algorithms, the EIFAsyn 
presented in [30,31], can be applied for the localization problem or a real problem. With that purpose, 
we modify the algorithm slightly in order to take into account some of the particularities of the problem, 
and we analyze which of the non-linear sensors that provide measurements about the robot position 
require to have its sensorial information recalculated when the algorithm receives older measurements 
out-of-sequence. After determining those necessities, we successfully test the behavior of the adapted 
OOS EIF, making it responsible for estimating the robot location used in all the other algorithms that 
form part of the complex control software of a real robot. 

Finally, this paper also compares the performance, computational cost and memory requirements of 
EIFAsyn with two of the other techniques that have been selected using the characteristics determined 
in our thoughtful analysis. The results of the comparison show that the selected algorithm EIFAsyn is 
the best choice for the localization problem regarding its memory and computational cost and one of the 
best options regarding its performance. 
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Appendix 

In the following equations, we show the Jacobians, F t) t-i = (Jf) x (xt-i\t-i7 u t,t-i, t, t — 1) and 
Hs,k — (Jh s ) x (xk\k-i, k), of the transition and measurement models. Additionally, the information in 
Table Al highlights some relevant aspects that have been considered when implementing the OOS PF 
and EnKF variants, shortly describing the problem in the first column, determining in which filters has 
to be considered in the second and third, and introducing the idea behind the solution in the last. 
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Table Al. Relevant additional aspects to implement the OOS PFs and EnKFs. 



Problem Description 
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EnKFs 


Solution 
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of angular variables 






coordinates of the angle [54] 


Resampling step 


X 




Carrying it out optionally, based on the number of effective particles [55] 


The existence of 


X 




Assigning a 0 weight to those particles from which the object can not be 


a sonar prediction 






observed, unless no observation can be predicted from any particle 


depends on the state 
orientation 




X 


Assimilating the sonar measurement only if an object can be observed from 
all the particles of the ensemble 
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